package com.evolved.automata.experimental;
import java.util.LinkedList;

import com.evolved.automata.AITools;
import com.evolved.automata.WeightedValue;
import com.evolved.automata.experimental.bots.*;

public class SRV1SensoryMotorProvider implements SensoryMotorDataProvider {
	
	private final int LEFT_BACK=0;
	private final int LEFT_STOP=1;
	private final int LEFT_FORWARD=2;
	private final int RIGHT_BACK=0;
	private final int RIGHT_STOP=1;
	private final int RIGHT_FORWARD=2;
	
	
	private static boolean s_DebugModeP=false;
	private static int s_DebugCounter=0;
	private static int[] s_DebugDataCounter = new int[]{90,80,78,75,60,50,10,2,34,40,35,45,50};
	SRV1Robot j_Robot;
	private int j_BasePower=50;
	private boolean j_ActiveRecovery=false;
	public SRV1SensoryMotorProvider(String ip, int port)
	{
		j_Robot = new SRV1Robot(ip,port);
	}
	
	public SRV1SensoryMotorProvider()
	{
		j_Robot = new SRV1Robot();
	}
	
	public SRV1SensoryMotorProvider(boolean recovery)
	{
		j_ActiveRecovery=recovery;
		j_Robot = new SRV1Robot();
	}
	
	public int[] GetSensoryData()
	{
		if (s_DebugModeP)
		{
			
			return new int[]{s_DebugDataCounter[(s_DebugCounter++) % s_DebugDataCounter.length ]};
		}
		else
		{
			int dist=-1, maxAttempts=10;
			double sample;
			dist= j_Robot.GetForwardDistance();
			int[] actions = new int[]{SRV1Robot.FORWARD_LEFT, SRV1Robot.FORWARD_RIGHT,SRV1Robot.BACKWARD_LEFT, SRV1Robot.BACKWARD_RIGHT};
			if (j_ActiveRecovery)
			{
				for (int i=0;i<maxAttempts;i++)
				{
					
					if (dist!=-1)
						break;
					sample=4*Math.random();
					j_Robot.MoveRobotTotal(actions[(int)sample], j_BasePower);
					
					dist= j_Robot.GetForwardDistance();
				}
			}
			return new int[]{dist};
		}
	}
	
	public String GetTypeName()
	{
		return "SRV1SensoryMotorProvider";
	}
	
	public int[] GetStateBaseRadii()
	{
		return new int[]{100};
	}
	
	
	public int[] GetActionBaseRadii()
	{
		// TODO: Change this back to 25 at some point
		//return new int[]{25};
		return new int[]{3,3};
	}
	
	public int[] GetInitializationActions()
	{
		return new int[]{SRV1Robot.FORWARD};
	}
	
	public static void EnableDebugMode()
	{
		s_DebugModeP=true;
		s_DebugCounter=0;
	}
	
	public static void DisableDebugMode()
	{
		s_DebugModeP=false;
	}
	
	public LinkedList<WeightedValue<Integer>> GetActionOptions(){
		LinkedList<WeightedValue<Integer>> actions = new LinkedList<WeightedValue<Integer>>();
		actions.add(new WeightedValue<Integer>(new Integer(SRV1Robot.LEFT),10));
		actions.add(new WeightedValue<Integer>(new Integer(SRV1Robot.RIGHT),10));
		actions.add(new WeightedValue<Integer>(new Integer(SRV1Robot.BACK),10));
		actions.add(new WeightedValue<Integer>(new Integer(SRV1Robot.FORWARD),10));
		actions.add(new WeightedValue<Integer>(new Integer(SRV1Robot.FORWARD_LEFT),10));
		actions.add(new WeightedValue<Integer>(new Integer(SRV1Robot.FORWARD_RIGHT),10));
		actions.add(new WeightedValue<Integer>(new Integer(SRV1Robot.BACKWARD_LEFT),10));
		actions.add(new WeightedValue<Integer>(new Integer(SRV1Robot.BACKWARD_RIGHT),10));
		actions.add(new WeightedValue<Integer>(new Integer(SRV1Robot.STOP),10));
		return actions;
	}
	
	private int MapCanonicalToPhysicalAction(int canonicalAction)
	{
		if (canonicalAction==CompressAction(new int[]{LEFT_BACK,RIGHT_BACK}))
			return SRV1Robot.BACK;
		else if (canonicalAction==CompressAction(new int[]{LEFT_FORWARD,RIGHT_FORWARD}))
			return SRV1Robot.FORWARD;
		else if (canonicalAction==CompressAction(new int[]{LEFT_FORWARD,RIGHT_BACK}))
			return SRV1Robot.RIGHT;
		else if (canonicalAction==CompressAction(new int[]{LEFT_BACK,RIGHT_FORWARD}))
			return SRV1Robot.LEFT;//
		else if (canonicalAction==CompressAction(new int[]{LEFT_STOP,RIGHT_BACK}))
			return SRV1Robot.BACKWARD_LEFT;
		else if (canonicalAction==CompressAction(new int[]{LEFT_BACK,RIGHT_STOP}))
			return SRV1Robot.BACKWARD_RIGHT;
		else if (canonicalAction==CompressAction(new int[]{LEFT_FORWARD,RIGHT_STOP}))
			return SRV1Robot.FORWARD_RIGHT;
		else if (canonicalAction==CompressAction(new int[]{LEFT_STOP,RIGHT_FORWARD}))
			return SRV1Robot.FORWARD_LEFT;
		else
			return SRV1Robot.STOP;
	}
	
	public int MapPhysicalToCanonicalAction(int physicalAction)
	{
		switch (physicalAction)
		{
			case SRV1Robot.BACK:
				return CompressAction(new int[]{LEFT_BACK,RIGHT_BACK});
			case SRV1Robot.FORWARD:
				return CompressAction(new int[]{LEFT_FORWARD,RIGHT_FORWARD});
			case SRV1Robot.RIGHT:
				return CompressAction(new int[]{LEFT_FORWARD,RIGHT_BACK});
			case SRV1Robot.LEFT:
				return CompressAction(new int[]{LEFT_BACK,RIGHT_FORWARD});
			case SRV1Robot.BACKWARD_LEFT:
				return CompressAction(new int[]{LEFT_STOP,RIGHT_BACK});
			case SRV1Robot.BACKWARD_RIGHT:
				return CompressAction(new int[]{LEFT_BACK,RIGHT_STOP});
			case SRV1Robot.FORWARD_RIGHT:
				return CompressAction(new int[]{LEFT_FORWARD,RIGHT_STOP});
			case SRV1Robot.FORWARD_LEFT:
				return CompressAction(new int[]{LEFT_STOP,RIGHT_FORWARD});
			default:
				return CompressAction(new int[]{LEFT_STOP,RIGHT_STOP});
		}
	}
	
	
	
	public int[] ExpandAction(int actionId)
	{
		return AITools.ReverseMapIndexToVector(MapPhysicalToCanonicalAction(actionId), GetActionBaseRadii());
	}
	
	public int CompressAction(int[] actionAttributions)
	{
		return AITools.MapDiscreteVectorToInt(actionAttributions, GetActionBaseRadii());
	}
	
	
	public double[] GetBaseSensoryInferentialFreedom()
	{
		return new double[]{.05,0.5,.5};
	}
	
	public double[] GetBaseClusterRangeFraction()
	{
		return new double[]{.15,0.5,0.5};
	}
	public LinkedList<WeightedValue<Integer>> GetPriorActionTendencies()
	{
		LinkedList<WeightedValue<Integer>> options = new LinkedList<WeightedValue<Integer>>();
		options.add(new WeightedValue<Integer>(SRV1Robot.FORWARD, 50));
		options.add(new WeightedValue<Integer>(SRV1Robot.LEFT, 20));
		options.add(new WeightedValue<Integer>(SRV1Robot.RIGHT, 20));
		return options;
	}
	
	public void ApplyAction(int[] action)
	{
		if ((action!=null)&&(!s_DebugModeP))
		{
			j_Robot.MoveSimple(CompressAction(action),j_BasePower);
			
		}
		
	}
	
	public void ApplyAction(int canonicalAction)
	{
		if ((canonicalAction>=0)&&(!s_DebugModeP))
		{
			j_Robot.MoveSimple(MapCanonicalToPhysicalAction(canonicalAction),j_BasePower);
			
		}
		
	}
	
	
	public void Start()
	{
		
	}
	public void Stop()
	{
		
	}

	@Override
	public int GetCriticalNegUtilityThreshold() {
		// TODO Auto-generated method stub
		return 7;
	}

	@Override
	public int GetLifeStepSize() {
		// TODO Auto-generated method stub
		return 10;
	}

	@Override
	public int GetMaxLifeSpan() {
		// TODO Auto-generated method stub
		return 200;
	}
}
